// ibvs_control.cpp
#include "servo_visual/ibvs_control.hpp"

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <memory>
#include <deque>
#include <std_msgs/msg/float64.hpp>
#include <vector>
#include <algorithm>
#include <optional>
#include <cmath>
#include <mutex>                // ★ 新增：互斥锁
#include <rcl_interfaces/msg/set_parameters_result.hpp>

// ================== 默认参数（见 ibvs_control.hpp） ==================
// 这里不重复定义，保持你现有头文件里的默认值

namespace servo_visual {
  IBVSControl::IBVSControl()
      : rclcpp::Node("visual_control_node"),
        steady_clock_(RCL_STEADY_TIME) // 用稳态时钟避免时间源不一致
  {
      RCLCPP_INFO(this->get_logger(), "IBVSControl 节点启动");

      // --- Callback Groups（★关键：把订阅/定时器放独立分组，便于多线程并发） ---
      cbg_io_   = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
      cbg_timer_= this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

      // --- 外置参数（使用头文件默认值） ---
      Kp_ = this->declare_parameter<double>("Kp", DEFAULT_KP);
      Ki_ = this->declare_parameter<double>("Ki", DEFAULT_KI);
      Kd_ = this->declare_parameter<double>("Kd", DEFAULT_KD);

      Kp_z_ = this->declare_parameter<double>("Kp_z", DEFAULT_KP_Z);
      Ki_z_ = this->declare_parameter<double>("Ki_z", DEFAULT_KI_Z);
      Kd_z_ = this->declare_parameter<double>("Kd_z", DEFAULT_KD_Z);

      // X轴（由“深度 z(米)”误差控制 linear.x）
      Kp_x_ = this->declare_parameter<double>("Kp_x", DEFAULT_KP_X);
      Ki_x_ = this->declare_parameter<double>("Ki_x", DEFAULT_KI_X);
      Kd_x_ = this->declare_parameter<double>("Kd_x", DEFAULT_KD_X);
      target_depth_m_ = this->declare_parameter<double>("target_depth_m", DEFAULT_TARGET_DEPTH_M);
      error_threshold_depth_m_ = this->declare_parameter<double>("error_threshold_depth_m", DEFAULT_ERROR_THRESHOLD_DEPTH_M);

      // 通用
      control_dt_ = this->declare_parameter<double>("control_dt", DEFAULT_CONTROL_DT);
      error_threshold_x_ = this->declare_parameter<double>("error_threshold_x", DEFAULT_ERROR_THRESHOLD_X);
      error_threshold_y_ = this->declare_parameter<double>("error_threshold_y", DEFAULT_ERROR_THRESHOLD_Y);

      // 滤波窗口（整数）
      raw_win_x_ = static_cast<size_t>(this->declare_parameter<int>("raw_window_x", static_cast<int>(DEFAULT_RAW_WIN_X)));
      raw_win_y_ = static_cast<size_t>(this->declare_parameter<int>("raw_window_y", static_cast<int>(DEFAULT_RAW_WIN_Y)));
      raw_win_z_ = static_cast<size_t>(this->declare_parameter<int>("raw_window_z", static_cast<int>(DEFAULT_RAW_WIN_Z)));
      med_win_x_ = static_cast<size_t>(this->declare_parameter<int>("med_window_x", static_cast<int>(DEFAULT_MED_WIN_X)));
      med_win_y_ = static_cast<size_t>(this->declare_parameter<int>("med_window_y", static_cast<int>(DEFAULT_MED_WIN_Y)));
      med_win_z_ = static_cast<size_t>(this->declare_parameter<int>("med_window_z", static_cast<int>(DEFAULT_MED_WIN_Z)));

      // 阶段参数
      small_forward_speed_ = this->declare_parameter<double>("small_forward_speed", DEFAULT_SMALL_FORWARD_SPEED);
      phase1_end_   = this->declare_parameter<double>("phase1_end",   DEFAULT_PHASE1_END);
      phase2_end_   = this->declare_parameter<double>("phase2_end",   DEFAULT_PHASE2_END);
      phase3_begin_ = this->declare_parameter<double>("phase3_begin", DEFAULT_PHASE3_BEGIN);

      // 参数回调（★快速返回+加锁清空窗口，避免竞态）
      param_cb_handle_ = this->add_on_set_parameters_callback(
        [this](const std::vector<rclcpp::Parameter> &params) {
          bool reset_I_y = false, reset_D_y = false;
          bool reset_I_z = false, reset_D_z = false;
          bool reset_I_x = false, reset_D_x = false;

          bool clr_raw_x=false, clr_raw_y=false, clr_raw_z=false;
          bool clr_med_x=false, clr_med_y=false, clr_med_z=false;

          for (const auto &p : params) {
            const auto &name = p.get_name();
            if (name == "Kp") Kp_ = std::max(0.0, p.as_double());
            else if (name == "Ki") { Ki_ = std::max(0.0, p.as_double()); reset_I_y = true; }
            else if (name == "Kd") { Kd_ = std::max(0.0, p.as_double()); reset_D_y = true; }

            else if (name == "Kp_z") Kp_z_ = std::max(0.0, p.as_double());
            else if (name == "Ki_z") { Ki_z_ = std::max(0.0, p.as_double()); reset_I_z = true; }
            else if (name == "Kd_z") { Kd_z_ = std::max(0.0, p.as_double()); reset_D_z = true; }

            else if (name == "Kp_x") Kp_x_ = std::max(0.0, p.as_double());
            else if (name == "Ki_x") { Ki_x_ = std::max(0.0, p.as_double()); reset_I_x = true; }
            else if (name == "Kd_x") { Kd_x_ = std::max(0.0, p.as_double()); reset_D_x = true; }
            else if (name == "target_depth_m")          target_depth_m_          = std::max(0.0, p.as_double());
            else if (name == "error_threshold_depth_m") error_threshold_depth_m_ = std::max(0.0, p.as_double());

            else if (name == "control_dt")        control_dt_        = std::max(1e-6, p.as_double());
            else if (name == "error_threshold_x") error_threshold_x_ = std::max(0.0, p.as_double());
            else if (name == "error_threshold_y") error_threshold_y_ = std::max(0.0, p.as_double());

            else if (name == "small_forward_speed") small_forward_speed_ = p.as_double();
            else if (name == "phase1_end")   phase1_end_   = std::max(0.0, p.as_double());
            else if (name == "phase2_end")   phase2_end_   = std::max(0.0, p.as_double());
            else if (name == "phase3_begin") phase3_begin_ = std::max(0.0, p.as_double());

            // 窗口参数（整数）
            else if (name == "raw_window_x") { raw_win_x_ = std::max<size_t>(1, static_cast<size_t>(p.as_int())); clr_raw_x = true; }
            else if (name == "raw_window_y") { raw_win_y_ = std::max<size_t>(1, static_cast<size_t>(p.as_int())); clr_raw_y = true; }
            else if (name == "raw_window_z") { raw_win_z_ = std::max<size_t>(1, static_cast<size_t>(p.as_int())); clr_raw_z = true; }
            else if (name == "med_window_x") { med_win_x_ = std::max<size_t>(1, static_cast<size_t>(p.as_int())); clr_med_x = true; }
            else if (name == "med_window_y") { med_win_y_ = std::max<size_t>(1, static_cast<size_t>(p.as_int())); clr_med_y = true; }
            else if (name == "med_window_z") { med_win_z_ = std::max<size_t>(1, static_cast<size_t>(p.as_int())); clr_med_z = true; }
          }

          if (reset_I_y) pid_y_.i_term = 0.0;
          if (reset_D_y) pid_y_.prev_error.reset();
          if (reset_I_z) pid_z_.i_term = 0.0;
          if (reset_D_z) pid_z_.prev_error.reset();
          if (reset_I_x) pid_x_.i_term = 0.0;
          if (reset_D_x) pid_x_.prev_error.reset();

          // ★ 清空缓冲加锁，避免与订阅回调并发访问
          if (clr_raw_x || clr_med_x || clr_raw_y || clr_med_y || clr_raw_z || clr_med_z) {
            std::scoped_lock lk(buf_mtx_);
            if (clr_raw_x) { x_window_.clear(); med_x_window_.clear(); }
            if (clr_med_x) { med_x_window_.clear(); }
            if (clr_raw_y) { y_window_.clear(); med_y_window_.clear(); }
            if (clr_med_y) { med_y_window_.clear(); }
            if (clr_raw_z) { z_window_.clear(); med_z_window_.clear(); }
            if (clr_med_z) { med_z_window_.clear(); }
          }

          rcl_interfaces::msg::SetParametersResult res;
          res.successful = true;
          res.reason = "ok";
          return res;
        });

      // TF
      tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
      tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

      // 发布器
      pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(
        "visual_control_node/delta_twist_cmds", 10);

      ibvs_x_avg_pub_ = this->create_publisher<std_msgs::msg::Float64>("/ibvs_x_avg", 10);
      ibvs_y_avg_pub_ = this->create_publisher<std_msgs::msg::Float64>("/ibvs_y_avg", 10);
      ibvs_z_avg_pub_ = this->create_publisher<std_msgs::msg::Float64>("/ibvs_z_avg", 10);

      // 订阅 /result3_topic（放入 IO 分组）
      rclcpp::SubscriptionOptions sub_opts;
      sub_opts.callback_group = cbg_io_;
      sub_ = this->create_subscription<geometry_msgs::msg::Point>(
        "/result3_topic", 10,
        std::bind(&IBVSControl::on_result3, this, std::placeholders::_1),
        sub_opts);
  }

  void IBVSControl::initialize() {
      setupPlanningSceneMonitor();
      setupServo();       // ★ 只在这里启动 Servo，不在订阅回调里再启动

      // 启动阶段控制定时器（使用稳态时钟计时）
      start_time_steady_   = steady_clock_.now();
      last_pid_time_steady_= start_time_steady_;

      // 定时器放入独立分组，利于并发
      control_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(100),
        std::bind(&IBVSControl::control_loop, this),
        cbg_timer_   // 直接把回调组传进来
      );

      RCLCPP_INFO(this->get_logger(), "IBVSControl 初始化完成，开始阶段化控制。");
  }

  bool IBVSControl::isFinished() const { return finished_; }

  void IBVSControl::setupPlanningSceneMonitor() {
      planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
        this->rclcpp::Node::shared_from_this(), "robot_description", tf_buffer_, "planning_scene_monitor");
      if (planning_scene_monitor_->getPlanningScene()) {
        planning_scene_monitor_->startStateMonitor("/joint_states");
        planning_scene_monitor_->setPlanningScenePublishingFrequency(25);
        planning_scene_monitor_->startPublishingPlanningScene(
          planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
          "/moveit_servo/publish_planning_scene");
        planning_scene_monitor_->startSceneMonitor();
        planning_scene_monitor_->providePlanningSceneService();
        RCLCPP_INFO(this->get_logger(), "规划场景监控器初始化完成");
      } else {
        RCLCPP_ERROR(this->get_logger(), "规划场景配置失败");
        throw std::runtime_error("Planning scene setup failed");
      }
  }

  void IBVSControl::setupServo() {
    auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(
      this->rclcpp::Node::shared_from_this());
      if (!servo_parameters) {
        RCLCPP_FATAL(this->get_logger(), "无法加载伺服参数");
        throw std::runtime_error("Failed to load servo parameters");
      }
      servo_ = std::make_unique<moveit_servo::Servo>(
        this->rclcpp::Node::shared_from_this(), servo_parameters, planning_scene_monitor_);      servo_->start();
      servo_started_ = true;
      RCLCPP_INFO(this->get_logger(), "MoveIt Servo 已启动。");
  }

  // 订阅回调（做滤波并更新三个轴的 avg）
  void IBVSControl::on_result3(const geometry_msgs::msg::Point::SharedPtr msg)
  {
      // 计算各窗口长度
      const size_t raw_x = std::max<size_t>(1, raw_win_x_);
      const size_t raw_y = std::max<size_t>(1, raw_win_y_);
      const size_t raw_z = std::max<size_t>(1, raw_win_z_);
      const size_t med_x = std::max<size_t>(1, med_win_x_);
      const size_t med_y = std::max<size_t>(1, med_win_y_);
      const size_t med_z = std::max<size_t>(1, med_win_z_);

      double x_avg, y_avg, z_avg;

      {
        // ★ 加锁保护窗口与均值计算，避免与参数回调清空窗口时的数据竞争
        std::scoped_lock lk(buf_mtx_);

        x_window_.push_back(msg->x);
        y_window_.push_back(msg->y);
        z_window_.push_back(msg->z);
        if (x_window_.size() > raw_x) x_window_.pop_front();
        if (y_window_.size() > raw_y) y_window_.pop_front();
        if (z_window_.size() > raw_z) z_window_.pop_front();

        auto median = [](const std::deque<double>& w) -> double {
          if (w.empty()) return 0.0;
          std::vector<double> s(w.begin(), w.end());
          std::sort(s.begin(), s.end());
          size_t n = s.size();
          return (n % 2) ? s[n/2] : (s[n/2-1] + s[n/2]) * 0.5;
        };

        const double x_med = median(x_window_);
        const double y_med = median(y_window_);
        const double z_med = median(z_window_);

        med_x_window_.push_back(x_med);
        med_y_window_.push_back(y_med);
        med_z_window_.push_back(z_med);
        if (med_x_window_.size() > med_x) med_x_window_.pop_front();
        if (med_y_window_.size() > med_y) med_y_window_.pop_front();
        if (med_z_window_.size() > med_z) med_z_window_.pop_front();

        auto mean_of = [](const std::deque<double>& w) {
          if (w.empty()) return 0.0;
          double s = 0.0; for (double v : w) s += v; return s / static_cast<double>(w.size());
        };

        last_x_avg_ = mean_of(med_x_window_);
        last_y_avg_ = mean_of(med_y_window_);
        last_z_avg_ = mean_of(med_z_window_);

        x_avg = last_x_avg_;
        y_avg = last_y_avg_;
        z_avg = last_z_avg_;
      }

      // 发布 avg（不持锁，缩短临界区）
      std_msgs::msg::Float64 m;
      m.data = x_avg; ibvs_x_avg_pub_->publish(m);
      m.data = y_avg; ibvs_y_avg_pub_->publish(m);
      m.data = z_avg; ibvs_z_avg_pub_->publish(m);
  }

  // 离散 PID（带 dt、积分限幅、误差死区）
  double IBVSControl::pid_control_dt_(PIDState &state, double target, double current, double dt,
                          double Kp, double Ki, double Kd, double error_threshold)
  {
      const double error = target - current;

      if (std::abs(error) < error_threshold) {
        state.i_term = 0.0;
        state.prev_error.reset();
        return 0.0;
      }

      constexpr double I_LIM = 0.05;
      state.i_term += Ki * error * dt;
      state.i_term = std::clamp(state.i_term, -I_LIM, I_LIM);

      double d_term = 0.0;
      if (state.prev_error) d_term = (error - *state.prev_error) / std::max(1e-6, dt);
      state.prev_error = error;

      return Kp * error + state.i_term + Kd * d_term;
  }

  // ============= 阶段控制定时器（核心逻辑） =============
  void IBVSControl::control_loop()
  {
      const rclcpp::Time now_steady = steady_clock_.now();
      const double elapsed = (now_steady - start_time_steady_).seconds();

      geometry_msgs::msg::TwistStamped twist;
      twist.header.stamp = this->get_clock()->now();  // 消息时间戳仍用节点时钟
      twist.header.frame_id = "base_link";

      double dt = (now_steady - last_pid_time_steady_).seconds();
      if (dt <= 0.0) dt = control_dt_;
      last_pid_time_steady_ = now_steady;

      // 读取最近均值（短暂读取，不加锁问题不大；如需严谨也可复制时加锁）
      double x_avg, y_avg, z_avg;
      {
        std::scoped_lock lk(buf_mtx_);
        x_avg = last_x_avg_;
        y_avg = last_y_avg_;
        z_avg = last_z_avg_;
      }

      if (elapsed < phase1_end_) {
        twist.twist.linear.x = -pid_control_dt_(pid_x_, target_depth_m_, z_avg, dt,
                                                Kp_x_, Ki_x_, Kd_x_, error_threshold_depth_m_);
        twist.twist.linear.y =  pid_control_dt_(pid_y_, 0.0, x_avg, dt,
                                                Kp_, Ki_, Kd_, error_threshold_x_);
        twist.twist.linear.z =  pid_control_dt_(pid_z_, 0.0, y_avg, dt,
                                                Kp_z_, Ki_z_, Kd_z_, error_threshold_y_);

        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
          "[Phase1 0~%.1fs] XYZ闭环: vx=%.4f vy=%.4f vz=%.4f | x_avg=%.2f y_avg=%.2f z_avg=%.3f",
          phase1_end_, twist.twist.linear.x, twist.twist.linear.y, twist.twist.linear.z,
          x_avg, y_avg, z_avg);
      }
      else if (elapsed < phase2_end_) {
        twist.twist.linear.x = small_forward_speed_;
        twist.twist.linear.y = 0.0;
        twist.twist.linear.z = 0.0;

        pid_x_.i_term = pid_y_.i_term = pid_z_.i_term = 0.0;
        pid_x_.prev_error.reset(); pid_y_.prev_error.reset(); pid_z_.prev_error.reset();

        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
          "[Phase2 %.1f~%.1fs] 前进恒速: vx=%.4f (m/s)", phase1_end_, phase2_end_, twist.twist.linear.x);
      }
      else if (elapsed < phase3_begin_) {
        twist.twist.linear.x = 0.0;
        twist.twist.linear.y = 0.0;
        twist.twist.linear.z = 0.0;
        RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000,
          "[Phase2.5 %.1f~%.1fs] 预停止: 全轴 0", phase2_end_, phase3_begin_);
      }
      else {
        twist.twist.linear.x = 0.0;
        twist.twist.linear.y = 0.0;
        twist.twist.linear.z = 0.0;

        if (!finished_) {
          finished_ = true;
          RCLCPP_INFO(this->get_logger(), "[Phase3 >%.1fs] 控制流程结束，置位 finished_=true", phase3_begin_);
          if (control_timer_) control_timer_->cancel();
        }
      }

      if (servo_started_ && !finished_) {
        pub_->publish(twist);
      }
  }
}

// -------------- 成员数据 --------------

// 提供 init() 以兼容现有 main() 结构
namespace ibvs_control {
  void init() {
    auto node = std::make_shared<servo_visual::IBVSControl>();
    node->initialize();

    // ★ 多线程执行器：参数服务与订阅/定时器可并发，避免“上一次发送未完成卡住”
    rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 2);
    exec.add_node(node);
    exec.spin();
  }
}
